﻿#include "StageControl.h"
#include <QDebug>
#include <Windows.h>

/// X 轴正方向向右
/// Y 轴正方向像后
/// Z 轴正方向向下
///
const int HOME_TIMER_INTERVAL = 100;

const int DEFAULT_XPPMM = 2000;
const int DEFAULT_YPPMM = 2000;
const int DEFAULT_ZPPMM = 2000;
const int DEFAULT_RULER_PULSE_PER_MM = 10000;

StageControl::StageControl(MotionController *card, QObject *parent)
    : QObject(parent),
      m_timerHome(nullptr),
      m_timerXYZMove(nullptr),
      m_allIsGoingHome(false)
{
    m_card = card;

    setDir(-1, -1, -1);
    setRuler(DEFAULT_RULER_PULSE_PER_MM, DEFAULT_RULER_PULSE_PER_MM, DEFAULT_RULER_PULSE_PER_MM);
    setStepSize(DEFAULT_XPPMM, DEFAULT_YPPMM, DEFAULT_ZPPMM);

//    m_card->setAcceleration(AXIS_X, 256000 * 2000);
//    m_card->setAcceleration(AXIS_Y, 256000 * 2000);
//    m_card->setAcceleration(AXIS_Z, 256000 * 2000);
//    m_card->setDeceleration(AXIS_X, 256000 * 2000);
//    m_card->setDeceleration(AXIS_Y, 256000 * 2000);
//    m_card->setDeceleration(AXIS_Z, 256000 * 2000);

    setSpeed(AXIS_X, 10.0);
    setSpeed(AXIS_Y, 10.0);
    setSpeed(AXIS_Z, 1.0);

    m_timerHome = new QTimer(this);
    m_timerHome->setInterval(HOME_TIMER_INTERVAL);
    connect(m_timerHome, SIGNAL(timeout()), this, SLOT(waitForGoHome()));

    m_timerXYZMove = new QTimer(this);
    m_timerXYZMove->setInterval(HOME_TIMER_INTERVAL);
    connect(m_timerXYZMove, SIGNAL(timeout()), this, SLOT(waitForMoveToPos()));

    connect(this, SIGNAL(updateEncoder(double,double)), &m_LEC, SLOT(updateEncoder(double,double)));
    //
    x_END = 0;
    x_theoryPos2encodePos = 0;
    y_END = 0;
    y_theoryPos2encodePos = 0;
    z_END = 0;
    z_theoryPos2encodePos = 0;
    m_timerXYZPos = new QTimer(this);
}

void StageControl::attach(MotionController * card)
{
    m_card = card;
}

void StageControl::loadSettings(QSettings &settings)
{
    ledata_x.loadFromSettings(settings, "Encoder/AxisX_");
    ledata_x.transfer(&lec_x);
    ledata_y.loadFromSettings(settings, "Encoder/AxisY_");
    ledata_y.transfer(&lec_y);

    m_pulse_per_millimeter[AXIS_X] = settings.value("Stage/xPPMM", DEFAULT_XPPMM).toDouble();
    m_pulse_per_millimeter[AXIS_Y] = settings.value("Stage/yPPMM", DEFAULT_YPPMM).toDouble();
    m_pulse_per_millimeter[AXIS_Z] = settings.value("Stage/zPPMM", DEFAULT_ZPPMM).toDouble();

    m_ruler_pulse_per_millimeter[AXIS_X] = settings.value("Stage/xRulerPPMM", DEFAULT_RULER_PULSE_PER_MM).toDouble();
    m_ruler_pulse_per_millimeter[AXIS_Y] = settings.value("Stage/yRulerPPMM", DEFAULT_RULER_PULSE_PER_MM).toDouble();
    m_ruler_pulse_per_millimeter[AXIS_Z] = settings.value("Stage/zRulerPPMM", DEFAULT_RULER_PULSE_PER_MM).toDouble();

    int left = settings.value("Stage/DirLeft", -1).toInt();
    int forward = settings.value("Stage/DirForward", -1).toInt();
    int up = settings.value("Stage/DirUp", -1).toInt();
    setDir(left, forward, up);

    int xEncodePos = settings.value("Stage/xEncodePos", 0).toInt();
    int yEncodePos = settings.value("Stage/yEncodePos", 0).toInt();
    int zEncodePos = settings.value("Stage/zEncodePos", 0).toInt();
//    int wEncodePos = settings.value("Stage/wEncodePos", 0).toInt();
    int xTheoryPos = settings.value("Stage/xTheoryPos", 0).toInt();
    int yTheoryPos = settings.value("Stage/yTheoryPos", 0).toInt();
    int zTheoryPos = settings.value("Stage/zTheoryPos", 0).toInt();
//    int wTheoryPos = settings.value("Stage/wTheoryPos", 0).toInt();
    if(!m_card)
    {
        qCritical() << "Motion control card is not valid";
        return;
    }
    m_card->setEncoderPos(AXIS_X, xEncodePos);
    m_card->setEncoderPos(AXIS_Y, yEncodePos);
    m_card->setEncoderPos(AXIS_Z, zEncodePos);

    m_card->setTheoryPos(AXIS_X, xTheoryPos);
    m_card->setTheoryPos(AXIS_Y, yTheoryPos);
    m_card->setTheoryPos(AXIS_Z, zTheoryPos);
}

void StageControl::writeSettings(QSettings &settings)
{
    int xTheoryPos = m_card->readTheoryPos(AXIS_X);
    int yTheoryPos = m_card->readTheoryPos(AXIS_Y);
    int zTheoryPos = m_card->readTheoryPos(AXIS_Z);

    int xEncodePos = m_card->readEncodePos(AXIS_X);
    int yEncodePos = m_card->readEncodePos(AXIS_Y);
    int zEncodePos = m_card->readEncodePos(AXIS_Z);

    settings.setValue("Stage/xEncodePos", xEncodePos);
    settings.setValue("Stage/yEncodePos", yEncodePos);
    settings.setValue("Stage/zEncodePos", zEncodePos);

    settings.setValue("Stage/xTheoryPos", xTheoryPos);
    settings.setValue("Stage/yTheoryPos", yTheoryPos);
    settings.setValue("Stage/zTheoryPos", zTheoryPos);
}

void StageControl::setDir(int left, int forward, int up)
{
    m_dir_left = left;
    m_dir_right = -left;
    m_dir_forward = forward;
    m_dir_backward = -forward;
    m_dir_up = up;
    m_dir_down = -up;
}

void StageControl::setSpeed(unsigned short axis, double mm_per_second)
{
    if(m_card)
    {
        int speed = mm_per_second * m_pulse_per_millimeter[axis];
        m_speed[axis] = speed;
        m_card->setSpeed(axis, speed);
    }
}

void StageControl::setXSpeed(double mm_per_second)
{

    setSpeed(AXIS_X, mm_per_second);
}

void StageControl::setYSpeed(double mm_per_second)
{
    setSpeed(AXIS_Y, mm_per_second);
}

void StageControl::setZSpeed(double mm_per_second)
{
    setSpeed(AXIS_Z, mm_per_second);
}

void StageControl::setStepSize(double x_ppm, double y_ppm, double z_ppm)
{
    m_pulse_per_millimeter[AXIS_X] = x_ppm;
    m_pulse_per_millimeter[AXIS_Y] = y_ppm;
    m_pulse_per_millimeter[AXIS_Z] = z_ppm;
}

void StageControl::setRuler(int x_pulse, int y_pulse, int z_pulse)
{
    m_ruler_pulse_per_millimeter[AXIS_X] = x_pulse;
    m_ruler_pulse_per_millimeter[AXIS_Y] = y_pulse;
    m_ruler_pulse_per_millimeter[AXIS_Z] = z_pulse;
}

void StageControl::xyMoveToPos(double x_mm, double y_mm, double speed)
{
    int x_position = m_dir_right * x_mm * m_pulse_per_millimeter[AXIS_X];
    int y_position = m_dir_forward * y_mm * m_pulse_per_millimeter[AXIS_Y];
    int x_speed = speed * m_pulse_per_millimeter[AXIS_X];
    int y_speed = speed * m_pulse_per_millimeter[AXIS_Y];
    if(m_card)
    {
        m_card->setSpeed(AXIS_X, x_speed);
        m_card->setSpeed(AXIS_Y, y_speed);
        m_card->moveTo(AXIS_X, x_position);
        m_card->moveTo(AXIS_Y, y_position);

        m_xIsMoving = true;
        m_yIsMoving = true;
        if(!m_timerXYZMove->isActive())
        {
            m_timerXYZMove->start();
        }
    }
}

void StageControl::zMoveToPos(double z_mm, double speed )
{
    //qDebug() << "zMoveToPos z_mm:" << z_mm;
    int z_position = m_dir_down * z_mm * m_pulse_per_millimeter[AXIS_Z];
    int z_speed = speed * m_pulse_per_millimeter[AXIS_Z];
    if(m_card)
    {
        m_card->setSpeed(AXIS_Z, z_speed);
        m_card->moveTo(AXIS_Z, z_position);
        m_zIsMoving = true;
        //qDebug() << m_timerXYZMove->isActive();
        if(!m_timerXYZMove->isActive())
        {
            m_timerXYZMove->start();
        }
    }
}

void StageControl::xyMoveToRelativePos(double x_mm, double y_mm, double speed)
{
    double xRuler, yRuler;
    encode2ruler(m_encodePos_mm[AXIS_X], m_encodePos_mm[AXIS_Y], xRuler, yRuler);
    ruler2encode(xRuler + x_mm, yRuler + y_mm, x_END, y_END);
    updatetheoryPos2encodePos();

    qDebug() << "m_encodePos_mm[AXIS_X] + x_mm - x_END =" << m_encodePos_mm[AXIS_X] + x_mm - x_END;

    if(m_card)
    {
        m_card->setSpeed(AXIS_X, speed * m_pulse_per_millimeter[AXIS_X]);
        m_card->setSpeed(AXIS_Y, speed * m_pulse_per_millimeter[AXIS_Y]);
        m_card->moveSteps(AXIS_X, m_dir_right * x_theoryPos2encodePos * m_pulse_per_millimeter[AXIS_X]);
        m_card->moveSteps(AXIS_Y, m_dir_forward * y_theoryPos2encodePos * m_pulse_per_millimeter[AXIS_Y]);
        m_xIsMoving = true;
        m_yIsMoving = true;
        if(!m_timerXYZMove->isActive())
        {
            m_timerXYZMove->start();
        }
    }
}

void StageControl::zMoveToRelativePos(double z_mm, double speed)
{
    int z_position = m_dir_down * z_mm * m_pulse_per_millimeter[AXIS_Z];
    int z_speed = speed * m_pulse_per_millimeter[AXIS_Z];
    if(m_card)
    {
        m_card->setSpeed(AXIS_Z, z_speed);
        m_card->moveSteps(AXIS_Z, z_position);
        m_zIsMoving = true;
        if(!m_timerXYZMove->isActive())
        {
            m_timerXYZMove->start();
        }
    }
}

///@brief moveBackward 显微镜向后运动
void StageControl::moveBackward()
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    m_card->move(AXIS_Y, m_dir_backward);
}

void StageControl::moveBackward(double speed)
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    setSpeed(AXIS_Y, speed);
    m_card->move(AXIS_Y, m_dir_backward);
}
///@brief moveForward  显微镜向后运动
void StageControl::moveForward()
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    m_card->move(AXIS_Y, m_dir_forward);
}
void StageControl::moveForward(double speed)
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    setSpeed(AXIS_Y, speed);
    m_card->move(AXIS_Y, m_dir_forward);
}
///@brief moveUp  显微镜向上运动
void StageControl::moveUp()
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    m_card->move(AXIS_Z, m_dir_up);
}
void StageControl::moveUp(double speed)
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    setSpeed(AXIS_Z, speed);
    m_card->move(AXIS_Z, m_dir_up);
}
///@brief moveDown  显微镜向下运动
void StageControl::moveDown()
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    m_card->move(AXIS_Z, m_dir_down);
}
void StageControl::moveDown(double speed)
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    setSpeed(AXIS_Z, speed);
    m_card->move(AXIS_Z, m_dir_down);
}
///@brief moveLeft  显微镜向左运动
void StageControl::moveLeft()
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    m_card->move(AXIS_X, m_dir_left);
}

void StageControl::moveLeft(double speed)
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    setSpeed(AXIS_X, speed);
    m_card->move(AXIS_X, m_dir_left);
}
///@brief moveRight 显微镜向右运动
void StageControl::moveRight()
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    m_card->move(AXIS_X, m_dir_right);
}
void StageControl::moveRight(double speed)
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    setSpeed(AXIS_X, speed);

    m_card->move(AXIS_X, m_dir_right);
}

///@brief stopX 停止 X 轴运动
void StageControl::stopX()
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    qDebug() << "stop(AXIS_X)";
    m_card->stop(AXIS_X);
}

///@brief stopY 停止 Y 轴运动
void StageControl::stopY()
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    qDebug() << "stop(AXIS_Y)";
    m_card->stop(AXIS_Y);
}

///@brief stopZ 停止 Z 轴运动
void StageControl::stopZ()
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    m_card->stop(AXIS_Z);
}

///@brief stop 停止所有运动
void StageControl::stop()
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    m_timerXYZPos->stop();
    disconnect(m_timerXYZPos, 0, 0, 0);

    m_card->stop(AXIS_X);
    m_card->stop(AXIS_Y);
    m_card->stop(AXIS_Z);
}

///@brief goHomeX X 轴回到零位
void StageControl::goHomeX()
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    //qDebug() << "m_card->goHome(AXIS_X, 50000)";
    m_card->goHome(AXIS_X, 40000);
    m_xIsGoingHome = true;
    if(!m_timerHome->isActive())
    {
        m_timerHome->start();
    }
}

///@brief goHomeY Y 轴回到零位
void StageControl::goHomeY()
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    m_card->goHome(AXIS_Y, 40000);
    m_yIsGoingHome = true;
    if(!m_timerHome->isActive())
    {
        m_timerHome->start();
    }
}

///@brief goHomeZ Z 轴回到零位
void StageControl::goHomeZ()
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    //moveUp(5.0);
    m_card->goHome(AXIS_Z, 40000);
    m_zIsGoingHome = true;
    if(!m_timerHome->isActive())
    {
        m_timerHome->start();
    }
}

void StageControl::goHome()
{
    if(!m_card)
    {
        qWarning() << "Motion control card is not valid";
        return;
    }
    m_allIsGoingHome = true;
    goHomeZ();
}

void StageControl::waitForGoHome()
{
    bool xIsStop, yIsStop, zIsStop, wIsStop;
    xIsStop = !(m_card->isMoving(AXIS_X));
    yIsStop = !(m_card->isMoving(AXIS_Y));
    zIsStop = !(m_card->isMoving(AXIS_Z));
    wIsStop = !(m_card->isMoving(AXIS_W));


    if (xIsStop && m_xIsGoingHome)
    {
        m_xIsGoingHome = false;
        Sleep(2000);
        m_card->setTheoryPos(AXIS_X, 0);
        m_card->setEncoderPos(AXIS_X, 0);
        emit xGoHomeFinish();
    }
    if (yIsStop && m_yIsGoingHome)
    {
        m_yIsGoingHome = false;
        Sleep(1500);
        m_card->setTheoryPos(AXIS_Y, 0);
        m_card->setEncoderPos(AXIS_Y, 0);
        emit yGoHomeFinish();
    }
    if (zIsStop && m_zIsGoingHome)
    {
        m_zIsGoingHome = false;
        Sleep(1500);
        m_card->setTheoryPos(AXIS_Z, 0);
        m_card->setEncoderPos(AXIS_Z, 0);
        emit zGoHomeFinish();
        if(m_allIsGoingHome)
        {
            goHomeX();
            goHomeY();
            return;
        }
    }
    if (wIsStop && m_wIsGoingHome)
    {
        m_wIsGoingHome = false;

        m_card->setTheoryPos(AXIS_W, 0);
        m_card->setEncoderPos(AXIS_W, 0);
        emit wGoHomeFinish();
    }
    if(xIsStop && yIsStop && zIsStop)
    {
        m_timerHome->stop();
        if(m_allIsGoingHome)
        {
            m_allIsGoingHome = false;
            qDebug() << "emit GoHomeFinish();";
            emit GoHomeFinish();
        }
    }
}

void StageControl::waitForMoveToPos()
{
    bool xIsStop, yIsStop, zIsStop;
    xIsStop = !(m_card->isMoving(AXIS_X));
    yIsStop = !(m_card->isMoving(AXIS_Y));
    zIsStop = !(m_card->isMoving(AXIS_Z));

    //qDebug() << "zIsStop:" << zIsStop;
    if( xIsStop && m_xIsMoving )
    {
        m_xIsMoving = false;
        emit xMoveFinish();
    }
    if( yIsStop && m_yIsMoving )
    {
        m_yIsMoving = false;
        emit yMoveFinish();
    }
    if( zIsStop && m_zIsMoving )
    {
        m_zIsMoving = false;
        qDebug() << "zMoveFinished";     
        emit zMoveFinish();
    }
    if(xIsStop && yIsStop && zIsStop)
    {
        //m_timerXYZMove->stop();
    }
}

void StageControl::moveToRelativePos(double x_mm, double y_mm, double z_mm, double speed)
{
    double xRuler, yRuler;
    encode2ruler(m_encodePos_mm[AXIS_X], m_encodePos_mm[AXIS_Y], xRuler, yRuler);
    ruler2encode(xRuler + x_mm, yRuler + y_mm, x_END, y_END);
    z_END = m_encodePos_mm[AXIS_Z] + z_mm;
    updatetheoryPos2encodePos();
    if(m_card)
    {
        m_card->setSpeed(AXIS_X, speed * m_pulse_per_millimeter[AXIS_X]);
        m_card->setSpeed(AXIS_Y, speed * m_pulse_per_millimeter[AXIS_Y]);
        m_card->moveSteps(AXIS_X, m_dir_right * x_theoryPos2encodePos * m_pulse_per_millimeter[AXIS_X]);
        m_card->moveSteps(AXIS_Y, m_dir_forward * y_theoryPos2encodePos * m_pulse_per_millimeter[AXIS_Y]);
        m_card->moveSteps(AXIS_Z, m_dir_down * z_theoryPos2encodePos * m_pulse_per_millimeter[AXIS_Z]);
        m_count = COUNT;
        m_timerXYZPos->setInterval(50);
        connect(m_timerXYZPos, SIGNAL(timeout()), this, SLOT(waitForPos()));
        m_timerXYZPos->start();
    }
}

void StageControl::moveToPos(double x_mm, double y_mm, double z_mm, double speed)
{
    qDebug() << "moveToPos by speed:" << x_mm << y_mm << z_mm << speed;
    ruler2encode(x_mm, y_mm, x_END, y_END);
    z_END = z_mm;
    updatetheoryPos2encodePos();
    if(m_card)
    {
        m_card->setSpeed(AXIS_X, speed * m_pulse_per_millimeter[AXIS_X]);
        m_card->setSpeed(AXIS_Y, speed * m_pulse_per_millimeter[AXIS_Y]);
        m_card->setSpeed(AXIS_Z, speed / 10 * m_pulse_per_millimeter[AXIS_Z]);
        m_card->moveSteps(AXIS_X, m_dir_right * x_theoryPos2encodePos * m_pulse_per_millimeter[AXIS_X]);
        m_card->moveSteps(AXIS_Y, m_dir_forward * y_theoryPos2encodePos * m_pulse_per_millimeter[AXIS_Y]);
        m_card->moveSteps(AXIS_Z, m_dir_down * z_theoryPos2encodePos * m_pulse_per_millimeter[AXIS_Z]);
        m_count = COUNT;
        m_timerXYZPos->setInterval(50);
        connect(m_timerXYZPos, SIGNAL(timeout()), this, SLOT(waitForPos()));
        m_timerXYZPos->start();
    }
}

void StageControl::waitForPos()
{
    if(m_card->isMoving(AXIS_X) == false && m_card->isMoving(AXIS_Y) == false && m_card->isMoving(AXIS_Z) == false)
    {
        //qDebug() << "m_count:" << m_count;
        updatetheoryPos2encodePos();
//        if(fabs(z_theoryPos2encodePos) > 0.010)
//        {
//            m_card->moveSteps(AXIS_Z, m_dir_down * z_theoryPos2encodePos * m_pulse_per_millimeter[AXIS_Z]);
//            return;
//        }
        if(fabs(x_theoryPos2encodePos) > 0.001 || fabs(y_theoryPos2encodePos) > 0.001)
        {
            return;
        }
        if(m_count == 0)
        {
            m_count = COUNT;
            m_timerXYZPos->stop();
            disconnect(m_timerXYZPos, 0, 0, 0);
//            qDebug() << "x_theoryPos2encodePos:" << x_theoryPos2encodePos;
//            qDebug() << "y_theoryPos2encodePos:" << y_theoryPos2encodePos;
//            qDebug() << "z_theoryPos2encodePos:" << z_theoryPos2encodePos;
            emit moveToRelativePos_achieve();
            emit moveToPos_achieve();
            return;
        }
        else
        {
            m_count--;
        }
        if(fabs(x_theoryPos2encodePos) <= 0.001 && fabs(y_theoryPos2encodePos) <= 0.001 && fabs(z_theoryPos2encodePos) <= 0.002)
        {
            m_timerXYZPos->stop();
            disconnect(m_timerXYZPos, 0, 0, 0);
            emit moveToRelativePos_achieve();
            emit moveToPos_achieve();
            return;
        }
//        if(fabs(x_theoryPos2encodePos) > 0.001)
//        {
//            m_card->moveSteps(AXIS_X, m_dir_right * x_theoryPos2encodePos * m_pulse_per_millimeter[AXIS_X]);
//        }
//        if(fabs(y_theoryPos2encodePos) > 0.001)
//        {
//            m_card->moveSteps(AXIS_Y, m_dir_forward * y_theoryPos2encodePos * m_pulse_per_millimeter[AXIS_Y]);
//        }
        if(fabs(z_theoryPos2encodePos) > 0.001)
        {
            m_card->moveSteps(AXIS_Z, m_dir_down * z_theoryPos2encodePos * m_pulse_per_millimeter[AXIS_Z]);
        }
    }
}

void StageControl::updatetheoryPos2encodePos()
{
    x_theoryPos2encodePos = x_END -  m_encodePos_mm[AXIS_X];
    y_theoryPos2encodePos = y_END -  m_encodePos_mm[AXIS_Y];
    z_theoryPos2encodePos = z_END -  m_encodePos_mm[AXIS_Z];
}

void StageControl::updatePos(double theoryPos_mm[], double encodePos_mm[])
{
    //qDebug() << m_card->readTheoryPos(AXIS_X);
    int xTheoryPos = m_dir_right * m_card->readTheoryPos(AXIS_X);
    int yTheoryPos = m_dir_forward * m_card->readTheoryPos(AXIS_Y);
    int zTheoryPos = m_dir_down * m_card->readTheoryPos(AXIS_Z);

    int xEncodePos = m_dir_right * m_card->readEncodePos(AXIS_X);
    int yEncodePos = m_dir_forward * m_card->readEncodePos(AXIS_Y);
    int zEncodePos = m_dir_down * m_card->readEncodePos(AXIS_Z);

    m_theoryPos_mm[AXIS_X] = xTheoryPos / m_pulse_per_millimeter[AXIS_X];
    m_theoryPos_mm[AXIS_Y] = yTheoryPos / m_pulse_per_millimeter[AXIS_Y];
    m_theoryPos_mm[AXIS_Z] = zTheoryPos / m_pulse_per_millimeter[AXIS_Z];

    m_encodePos_mm[AXIS_X] = xEncodePos / m_ruler_pulse_per_millimeter[AXIS_X];
    m_encodePos_mm[AXIS_Y] = yEncodePos / m_ruler_pulse_per_millimeter[AXIS_Y];
    m_encodePos_mm[AXIS_Z] = zEncodePos / m_ruler_pulse_per_millimeter[AXIS_Z];

    for(int i = 0; i < 4; i++)
    {
        theoryPos_mm[i] = m_theoryPos_mm[i];
        encodePos_mm[i] = m_encodePos_mm[i];
    }
    encode2ruler(m_encodePos_mm[AXIS_X], m_encodePos_mm[AXIS_Y], encodePos_mm[AXIS_X], encodePos_mm[AXIS_Y]);
    emit updateEncoder(m_encodePos_mm[AXIS_X], m_encodePos_mm[AXIS_Y]);
}

void StageControl::updateInputIO(bool io[])
{
    m_card->readIO(io);
}

void StageControl::encode2ruler(double xEncode, double yEncode, double &xRuler, double &yRuler)
{
    xRuler = lec_x.encoderPosToTruePos(xEncode);
    yRuler = lec_y.encoderPosToTruePos(yEncode);
//    qDebug() << "xEncode:" << xEncode << "  xRuler:" << xRuler;
//    qDebug() << "yEncode:" << yEncode << "  yRuler:" << yRuler;
}

void StageControl::ruler2encode(double xRuler, double yRuler, double &xEncode, double &yEncode)
{
    xEncode = lec_x.truePosToEncoderPos(xRuler);
    yEncode = lec_y.truePosToEncoderPos(yRuler);
}

void StageControl::LECalibrator_show()
{
    m_LEC.show();
//    QString path = QStandardPaths::writableLocation(QStandardPaths::AppConfigLocation);
//    QSettings settings( path + "/JHPhoto.ini", QSettings::IniFormat, this );
//    ledata_x.loadFromSettings(settings, "Encoder/AxisX_");
//    ledata_x.transfer(&lec_x);
//    ledata_y.loadFromSettings(settings, "Encoder/AxisY_");
//    ledata_y.transfer(&lec_y);
}
